{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# HybridFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/hybrid/doc/HybridFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "`HybridFactor` is the abstract base class for all factors involved in hybrid inference problems within GTSAM. It extends the standard `gtsam.Factor` interface to accommodate factors that may involve both discrete and continuous variables.\n",
        "\n",
        "Key features:\n",
        "*   Manages separate sets of continuous (`gtsam.Key`) and discrete (`gtsam.DiscreteKey`) variable keys.\n",
        "*   Provides methods to check the type of factor (discrete-only, continuous-only, or truly hybrid).\n",
        "*   Defines virtual methods like `errorTree` and `restrict` that derived classes must implement based on their specific nature.\n",
        "\n",
        "*   [`gtsam.HybridGaussianFactor`](HybridGaussianFactor.ipynb)\n",
        "*   [`gtsam.HybridGaussianConditional`](HybridGaussianConditional.ipynb) (inherits from HybridGaussianFactor)\n",
        "*   [`gtsam.HybridNonlinearFactor`](HybridNonlinearFactor.ipynb)\n",
        "*   [`gtsam.HybridConditional`](HybridConditional.ipynb) (wrapper class)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "\n",
        "from gtsam import HybridGaussianFactor, JacobianFactor, DecisionTreeFactor\n",
        "from gtsam.symbol_shorthand import X, D"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Basic Interface\n",
        "\n",
        "All hybrid factors provide methods to access their continuous and discrete keys and check their category."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Is Discrete? False\n",
            "Is Continuous? False\n",
            "Is Hybrid? True\n",
            "Number of Continuous Keys: 1\n",
            "Continuous Keys: [8646911284551352320]\n",
            "Discrete Keys: \n",
            "d0 2\n",
            "\n",
            "All Keys (Factor base): [8646911284551352320, 7205759403792793600]\n",
            "\n",
            "Hybrid Factor Example (HybridGaussianFactor):\n",
            "Hybrid [x0; d0]{\n",
            " Choice(d0) \n",
            " 0 Leaf :\n",
            "  A[x0] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 0 ]\n",
            "  Noise model: unit (1) \n",
            "scalar: 0\n",
            "\n",
            " 1 Leaf :\n",
            "  A[x0] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 1 ]\n",
            "  Noise model: unit (1) \n",
            "scalar: 0\n",
            "\n",
            "}\n"
          ]
        }
      ],
      "source": [
        "# Example using a derived class (HybridGaussianFactor)\n",
        "dk = (D(0), 2) # Discrete key D0 with cardinality 2\n",
        "gf0 = JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Unit.Create(1))\n",
        "gf1 = JacobianFactor(X(0), np.eye(1), np.ones(1), gtsam.noiseModel.Unit.Create(1))\n",
        "hybrid_factor = HybridGaussianFactor(dk, [gf0, gf1])\n",
        "\n",
        "# Access methods inherited from HybridFactor\n",
        "print(f\"Is Discrete? {hybrid_factor.isDiscrete()}\")\n",
        "print(f\"Is Continuous? {hybrid_factor.isContinuous()}\")\n",
        "print(f\"Is Hybrid? {hybrid_factor.isHybrid()}\")\n",
        "print(f\"Number of Continuous Keys: {hybrid_factor.nrContinuous()}\")\n",
        "print(f\"Continuous Keys: {hybrid_factor.continuousKeys()}\")\n",
        "print(f\"Discrete Keys: {hybrid_factor.discreteKeys()}\") # List of (key, card) pairs\n",
        "print(f\"All Keys (Factor base): {hybrid_factor.keys()}\") # Combined keys\n",
        "\n",
        "hybrid_factor.print(\"\\nHybrid Factor Example (HybridGaussianFactor):\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Virtual Methods (`errorTree`, `restrict`)\n",
        "\n",
        "Derived classes implement methods to handle the interaction between discrete and continuous parts.\n",
        "\n",
        "*   `errorTree(VectorValues)`: Calculates the error for the continuous part, returning an `AlgebraicDecisionTree` keyed by the discrete variables.\n",
        "*   `restrict(DiscreteValues)`: Creates a new factor (typically continuous-only) by fixing the discrete variables to a specific assignment."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Error Tree (AlgebraicDecisionTree):\n",
            "ErrorTree\n",
            " Leaf 0.125\n"
          ]
        }
      ],
      "source": [
        "# --- errorTree ---\n",
        "# Needs continuous values\n",
        "continuous_values = gtsam.VectorValues()\n",
        "continuous_values.insert(X(0), np.array([0.5]))\n",
        "continuous_values.insert(X(1), np.array([1.5]))\n",
        "\n",
        "adt = hybrid_factor.errorTree(continuous_values)\n",
        "print(\"\\nError Tree (AlgebraicDecisionTree):\")\n",
        "adt.print()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "id": "f21e8197",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Restricted Factor (Assignment D0=0):\n",
            "Continuous [x0]{\n",
            " Leaf :\n",
            "  A[x0] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 0 ]\n",
            "  Noise model: unit (1) \n",
            "scalar: 0\n",
            "\n",
            "}\n",
            "\n",
            "Restricted Factor (Assignment D0=1):\n",
            "Continuous [x0]{\n",
            " Leaf :\n",
            "  A[x0] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 1 ]\n",
            "  Noise model: unit (1) \n",
            "scalar: 0\n",
            "\n",
            "}\n"
          ]
        }
      ],
      "source": [
        "# --- restrict ---\n",
        "# Needs a discrete assignment\n",
        "assignment = gtsam.DiscreteValues()\n",
        "assignment[D(0)] = 0 # Choose the first Gaussian factor component\n",
        "\n",
        "restricted_factor = hybrid_factor.restrict(assignment)\n",
        "# The result is typically a shared_ptr[Factor], often a GaussianFactor\n",
        "restricted_factor.print(\"\\nRestricted Factor (Assignment D0=0):\")\n",
        "\n",
        "# Try restricting with the other assignment\n",
        "assignment[D(0)] = 1\n",
        "restricted_factor_1 = hybrid_factor.restrict(assignment)\n",
        "restricted_factor_1.print(\"\\nRestricted Factor (Assignment D0=1):\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
